%author: wxj233
%time: 2023.10.25 12:00
%function: 主要用于完成特征点仿真以及轨迹模拟，整个场景在二维平面上进行

clc;
clear;
clear Function;
close all;

% 3倍sigma点就是精度
p_d = 0.4/3;  % 测距精度0.4m
p_v = 0.02778/3;  % 测速精度0.0277m/s
p_theta = 0.3/180*pi/3; % 测角精度0.3°（近距离），远距离是0.1°。都是波束中心附近

% 初始化仿真器
% dt
dt = 0.072;
simulater = Simulater(dt);

% 产生第一个目标特征点群
% len, width, probabilitys, varargin{seed}
cluster1_1 = simulater.generateFeaturePoints(10, 2.2, [0.5,0.5,0.5,0.5], 1);
cluster1_2 = simulater.generateFeaturePoints(10, 2.2, [0.5,0.5], 2);
% 根据特征点群，产生第一条轨迹
% cluster, point0, v, a, T0, Times, id, sdr, sdvr, sdtheta, varargin[seed1, seed2]
[T1_1_1,rT_1_1_1, ep1_1_1, ev1_1_1, et1_1_1] = simulater.generateTrack(cluster1_1, [-150, 105], [10, -10], [0, 1], 0, [0, 10], 1, p_d, p_v, p_theta, 11, 12);  % [[t, r, theta, vr, x, y, vx, vy, id]; ...]
[T1_2_1,rT_1_2_1, ep1_2_1, ev1_2_1, et1_2_1] = simulater.generateTrack(cluster1_2, [-150, 105], [10, -10], [0, 1], 0, [0, 10], 1, p_d, p_v, p_theta, 21, 22);  % [[t, r, theta, vr, x, y, vx, vy, id]; ...]

[T1_1_2,rT_1_1_2, ep1_1_2, ev1_1_2, et1_1_2] = simulater.generateTrack(cluster1_1, ep1_1_1, ev1_1_1, [0, 0], et1_1_1, [dt, 10], 1, p_d, p_v, p_theta, 31, 32);
[T1_2_2,rT_1_2_2, ep1_2_2, ev1_2_2, et1_2_2] = simulater.generateTrack(cluster1_2, ep1_2_1, ev1_2_1, [0, 0], et1_2_1, [dt, 10], 1, p_d, p_v, p_theta, 46, 47);

[T1_1_3,rT_1_1_3, ep1_1_3, ev1_1_3, et1_1_3] = simulater.generateTrack(cluster1_1, ep1_1_2, ev1_1_2, [0, -1], et1_1_2, [dt, 10], 1, p_d, p_v, p_theta, 36, 37);
[T1_2_3,rT_1_2_3, ep1_2_3, ev1_2_3, et1_2_3] = simulater.generateTrack(cluster1_2, ep1_2_2, ev1_2_2, [0, -1], et1_2_2, [dt, 10], 1, p_d, p_v, p_theta, 41, 42);

rT1 = [(rT_1_1_1*4+rT_1_2_1*2)/6; (rT_1_1_2*4+rT_1_2_2*2)/6; (rT_1_1_3*4+rT_1_2_3*2)/6];

% 产生第二个目标特征点群
% len, width, probabilitys, varargin{seed}
cluster2_1 = simulater.generateFeaturePoints(5, 2.2, [0.5, 0.5, 0.5], 2);
% 根据特征点群，产生第二条轨迹
% cluster, point0, v, a, T0, Times, id, sdr, sdvr, sdtheta, varargin[seed1, seed2]
[T2_1,rT_2_1, ep2_1, ev2_1, et2_1] = simulater.generateTrack(cluster2_1, [-150, 0], [10, 10], [0, -1], 0, [0, 10], 2, p_d, p_v, p_theta, 51, 52);  % [[t, r, theta, vr, x, y, vx, vy, id]; ...]
[T2_2,rT_2_2, ep2_2, ev2_2, et2_2] = simulater.generateTrack(cluster2_1, ep2_1, ev2_1, [0, 0], et2_1, [dt, 10], 2, p_d, p_v, p_theta, 61, 62);
[T2_3,rT_2_3, ep2_3, ev2_3, et2_3] = simulater.generateTrack(cluster2_1, ep2_2, ev2_2, [0, 1], et2_2, [dt, 10], 2, p_d, p_v, p_theta, 61, 62);

rT2 = [rT_2_1;rT_2_2;rT_2_3];

% 产生第三个目标特征点群
% len, width, probabilitys, varargin{seed}
cluster3 = simulater.generateFeaturePoints(5, 2.2, [0.5, 0.5, 0.5,0.5], 4);
% % 根据特征点群，产生第三条轨迹
% cluster, point0, v, a, T0, Times, id, sdr, sdvr, sdtheta, varargin[seed1, seed2]
[T3_1,rT3, ep3_1, ev3_1, et3_1] = simulater.generateTrack(cluster3, [-150, 60], [10, 0], [0, 0], 0, [0, 30], 3, p_d, p_v, p_theta, 71, 72);  % [[t, r, theta, vr, x, y, vx, vy, id]; ...]


figure(1);
plot(rT1(:, 2), rT1(:, 3), 'DisplayName', "目标1");hold on;
plot(rT2(:, 2), rT2(:, 3), 'DisplayName', "目标2");hold on;
plot(rT3(:, 2), rT3(:, 3), 'DisplayName', "目标3");hold on;

title("目标真实轨迹信息");
xlabel("x(m)");
ylabel("y(m)");
legend;


save("rt1", "rT1");
save("rt2", "rT2");
save("rt3", "rT3");


